#include "test.h"

#include <cstdio>
#include <string>
#include <iostream>
#include <cmath>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/nonfree/nonfree.hpp>
//#include <opencv2/video/video.hpp>
#include "motiondetect.h"
#include "featurematch.h"
#include "timestamp.h"
#include "CSerial.h"
using namespace std;


using namespace cv;
Test::Test()
{
}
const char* test_keys =
{
    "{c| camera |-1   | camera number}"
    "{f| file   |false| use file as source}"
    "{1|        |stdfile   |source file}"
    "{w|wait    |-1   |go straight to the frame}"
    "{b|begin   | 0    |begin frame}"
};

Point2f sp1(120, 80), sp2(540, 320);
//Point2f sp1(376, 308), sp2(243, 200);
Point2f sp3;
Point2f rp1(0, 0), rp2(0, 0);
//Point2f rp1(232, 176), rp2(632, 303);
bool track = true;
bool match = false;
bool same = false;
int interval = 40;
void onMouse(int event, int x, int y, int, void*){
    static bool mouse_down = false;
    static bool right_mouse_down = false;
    switch(event){
    case CV_EVENT_MOUSEMOVE:
        if(mouse_down){
            if(x >= 0 && y >=0 && x != sp1.x && y != sp1.y){
                sp2.x = x;
                sp2.y = y;
            }
        }
        if(right_mouse_down){
            if(x >= 0 && y >=0 && x != rp1.x && y != rp1.y){
                rp2.x = x;
                rp2.y = y;
            }
        }
        break;
    case CV_EVENT_LBUTTONDOWN:
        track = false;
        mouse_down = true;
        if(x >= 0 && y >=0){
            sp1.x = x;
            sp1.y = y;
        }
        break;
    case CV_EVENT_LBUTTONUP:
        mouse_down = false;
        track = true;
        break;
    case CV_EVENT_RBUTTONDOWN:
        track = false;
        right_mouse_down = true;
        if(x >= 0 && y >= 0){
            rp1.x = x;
            rp1.y = y;
        }
        break;
    case CV_EVENT_RBUTTONUP:
        right_mouse_down = false;
        track = true;
        break;
    }
}

bool Test::selectMask(Mat mask, Rect area, Mat& out){
    Mat temp = Mat::zeros(mask.size(), CV_8U);
    temp(area).setTo(255);
    bitwise_and(mask, temp, out);
    Moments m = moments(out, true);
    if(m.m00 < 1){
        return false;
    }
    return true;
}

void Test::testComponents(int argc, const char **argv){
    //parse command line

    VideoCapture cap;
    CommandLineParser parser(argc, argv, test_keys);
    int device = parser.get<int>("camera");
    bool use_file = parser.get<bool>("file");
    string file_name = parser.get<string>("1");
    int wait_frame = parser.get<int>("w");
    bool wait = false;
    int begin_frame = parser.get<int>("b");
    if(wait_frame != -1){
        wait = true;
    }
    printf("device = %d\n", device);
    if(use_file){
        printf("filename = %s\n", file_name.c_str());
        cap.open(file_name);
    }else{
        cap.open(device);
    }
    Mat cur;
    Mat pre;
    Mat pre_pre;
    Mat interest;
    Mat diff;
    Mat ori_diff;
    Mat fg_mask;
    Mat fg_img;
    Mat warp_mat;
    Mat temp;

    TimeStamp cur_ts;
    TimeStamp pre_ts;
    bool first = true;

    namedWindow("cur", 1);
    setMouseCallback("cur", onMouse, 0);
//    namedWindow("pre", 1);
//    namedWindow("interest", 1);
//    namedWindow("diff", 1);
//    namedWindow("oridiff", 1);
//    namedWindow("fg_mask", 1);
    namedWindow("mask", 1);


    MotionDetect motion_detect;
    FeatureMatch feature_match;
    Rect area;
    Rect search_area;
    Rect found;
    int frame_count = 0;
    vector<Point2f> points;
    area = Rect(sp1, sp2);
    sp3.x = (sp1.x + sp2.x) / 2;
    sp3.y = (sp1.y + sp2.y) / 2;

    Mat mask;
    int select_frame = 0;
    for (;;){
        if(!same){
            cap>>cur;
            cur_ts.setNow();
        }
        if(cur.empty()){
            if(use_file){
                cap.open(file_name);
                pre = Mat();
                continue;
            }else{
                break;
            }
        }


        if(!same)
            frame_count ++;
        if(frame_count < begin_frame - 1)
            continue;
        if(!pre.empty() && pre.size() == cur.size()){
            cur.copyTo(temp);
            pre_ts = cur_ts;
            Scalar blue(255, 0 ,0);
            Scalar red(0, 0, 255);
            if(track){
                Rect obj_area;
                obj_area.x = area.x + area.width / 4;
                obj_area.y = area.y + area.height / 4;
                obj_area.width = area.width / 2;
                obj_area.height = area.height / 2;
                Mat selected_mask;

//                if(!match){
//                    if(same || motion_detect.detectMovingObjects(pre, cur, area, mask, 0, 3)){
//                        printf("motion detect cost %d ms\n", motion_detect.m_detect_cost);
//                        selectMask(mask, area, selected_mask);
//                        imshow("mask", selected_mask);
//                        Mat masked;
//                        cur.copyTo(masked, selected_mask);
//                        imshow("masked", masked);

//                        if(feature_match.detectSift(cur, selected_mask, TimeStamp())){
//                            printf("detect cost %d ms \n", feature_match.m_detect_cost);
//                        }
//                    }
//                }else{
//                    if(same || motion_detect.detectMovingObjects(pre, cur, search_area, mask, 0, 7)){
//                        printf("motion detect cost %d ms\n", motion_detect.m_detect_cost);
//                        selectMask(mask, search_area, selected_mask);
//                        imshow("mask", selected_mask);
//                        Mat masked;
//                        cur.copyTo(masked, selected_mask);
//                        imshow("masked", masked);
//                        if(feature_match.matchSift(cur, selected_mask, TimeStamp())){
//                            printf("match cost %d ms\n", feature_match.m_match_cost);
//                            imshow("match", feature_match.m_match_img);
//                        }
//                        if(frame_count >= wait_frame){
//                          imwrite("cur.jpg", temp);
//                          imwrite("masked.jpg", masked);
//                          imwrite("mask.jpg", mask);
//                          imwrite("match.jpg", feature_match.m_match_img);
//                        }
//                    }
//                }
//                if(!match){
//                    if(same || motion_detect.detectMovingObjects(pre, cur, area, mask, 1, 3)){
//                        printf("motion detect cost %d ms\n", motion_detect.m_detect_cost);
//                        selectMask(mask, area, selected_mask);
//                        imshow("mask", selected_mask);
//                        Mat masked;
//                        cur.copyTo(masked, selected_mask);
//                        imshow("masked", masked);

//                        if(feature_match.detectSurf(cur, selected_mask, TimeStamp())){
//                            printf("detect cost %d ms \n", feature_match.m_detect_cost);
//                        }
//                    }
//                }else{
//                    if(same || motion_detect.detectMovingObjects(pre, cur, search_area, mask, 1, 7)){
//                        printf("motion detect cost %d ms\n", motion_detect.m_detect_cost);
//                        selectMask(mask, search_area, selected_mask);
//                        imshow("mask", selected_mask);
//                        Mat masked;
//                        cur.copyTo(masked, selected_mask);
//                        imshow("masked", masked);

//                        if(feature_match.matchSurf(cur, selected_mask, TimeStamp())){
//                            printf("match cost %d ms\n", feature_match.m_match_cost);

//                        }
//                    }
//                }
//                if(!match){
//                    if(feature_match.detectSurf(cur, area)){
//                        printf("detect cost %d ms \n", feature_match.m_detect_cost);
//                        rectangle(temp, area, blue, 3 );
//                        rectangle(temp, search_area, red, 2);
//                        imshow("cur", temp);
//                    }
//                }else{
//                    if(feature_match.matchSurf(cur, search_area )){
//                        printf("match cost %d ms\n", feature_match.m_match_cost);
//                        rectangle(temp, area, blue, 3 );
//                        rectangle(temp, search_area, red, 2);
//                        imshow("cur", temp);
//                    }
//                }
//                if(!match){
//                    if(feature_match.detectSift(cur, area)){
//                        printf("detect cost %d ms \n", feature_match.m_detect_cost);
//                        rectangle(temp, area, blue, 3 );
//                        rectangle(temp, search_area, red, 2);
//                        imshow("cur", temp);
//                    }
//                }else{
//                    if(feature_match.matchSift(cur, search_area )){
//                        printf("match cost %d ms\n", feature_match.m_match_cost);
//                        rectangle(temp, area, blue, 3 );
//                        rectangle(temp, search_area, red, 2);
//                        imshow("cur", temp);
//                    }
//                }
//                if(same || motion_detect.detectOpticalFlow(pre, cur, area, mask, 0.3, 0, 0)){//optical
//                    printf("detect cost %d ms\n", motion_detect.m_adjust_cost);
//                    imshow("mask", mask);
//                    rectangle(temp, area, blue, 3 );
//                    imshow("cur", temp);
//                    Mat masked;
//                    cur.copyTo(masked, mask);
//                    imshow("masked", masked);
//                    if(frame_count == wait_frame){
//                        imwrite("cur.jpg", temp);
//                        imwrite("masked.jpg", masked);
//                        imwrite("mask.jpg", mask);
//                    }
//                }
//                if(motion_detect.detectMovingObjects(pre, cur, area, mask, 0, 0)){//background substrctor
//                    printf("detect cost %d ms\n", motion_detect.m_adjust_cost);
//                    imshow("mask", mask);
//                    rectangle(temp, area, blue, 3 );
//                    rectangle(temp, search_area, red, 2);
//                    imshow("cur", temp);
//                    Mat masked;
//                    cur.copyTo(masked, mask);
//                    imshow("masked", masked);
//                    if(frame_count == wait_frame){
//                        imwrite("cur.jpg", temp);
//                        imwrite("masked.jpg", masked);
//                        imwrite("mask.jpg", mask);

//                    }

//                    selectMask(mask, search_area);
//                    imshow("mask_arae", mask);
//                }
//                if(motion_detect.shakeAdjust(pre, cur, area, obj_area, mask)){//simple diff optical flow
//                    printf("adjust cost %d ms\n", motion_detect.m_adjust_cost);
//                    imshow("mask", mask);
//                    rectangle(temp, area, blue, 3 );
//                    imshow("cur", temp);
//                    Mat masked;
//                    cur.copyTo(masked, mask);
//                    imshow("masked", masked);
//                    if(frame_count >= wait_frame){
//                      imwrite("cur.jpg", temp);
//                      imwrite("masked.jpg", masked);
//                      imwrite("mask.jpg", mask);
//                    }
//                }
//                if(!pre_pre.empty()){
//                    if(same||motion_detect.temperalDifferencing(pre_pre, pre, cur, area, mask, 10, 0, 0)){//simple diff optical flow
//                        pre.copyTo(temp);
//                        printf("adjust cost %d ms\n", motion_detect.m_adjust_cost);
//                        imshow("mask", mask);
//                        rectangle(temp, area, blue, 3 );
//                        imshow("cur", temp);
//                        Mat masked;
//                        cur.copyTo(masked, mask);
//                        imshow("masked", masked);
//                        if(frame_count == wait_frame){
//                            imwrite("cur.jpg", temp);
//                            imwrite("masked.jpg", masked);
//                            imwrite("mask.jpg", mask);
//                        }
//                    }
//                }

//                if(same||motion_detect.shakeAdjust(pre, cur, area, obj_area, mask)){//simple diff optical flow
//                    printf("adjust cost %d ms\n", motion_detect.m_adjust_cost);
//                    imshow("mask", mask);
//                    rectangle(temp, area, blue, 3 );
//                    imshow("cur", temp);
//                    Mat masked;
//                    cur.copyTo(masked, mask);
//                    imshow("masked", masked);
//                    if(frame_count == wait_frame){
//                        imwrite("cur.jpg", temp);
////                        imwrite("masked.jpg", masked);
////                        imwrite("mask.jpg", mask);
//                    }
//                }

//                if(!first){//track
//                    if(motion_detect.trackObject(pre, pre_ts, temp, cur_ts, area, sp3, points)){
//                        printf("track cost %d ms\n", motion_detect.m_track_cost);
//                        Scalar green(0, 255, 0);
//                        circle(temp, sp3, 3, green);
//                        imshow("cur", temp);
//                    }
//                }
//                else{
//                    first = false;
//                    if(motion_detect.spreadInterestPoint(pre, pre_ts, temp, cur_ts, area, sp3, points)){
//                        printf("spread cost %d ms\n", motion_detect.m_calc_cost);
//                        Scalar red(0, 0, 255);

//                        cout<<"size = " << points.size()<<"\n";
//                        int min_x = 10000, min_y = 10000, max_x = 0, max_y = 0;
//                        circle(temp, points[0], 3, red);
//                        for(int i = 1; i < (int)points.size(); i++){
//                            if(points[i].x < min_x) min_x = points[i].x;
//                            if(points[i].x > max_x) max_x = points[i].x;
//                            if(points[i].y < min_y) min_y = points[i].y;
//                            if(points[i].y > max_y) max_y = points[i].y;
//                            circle(temp, points[i], 3, red);
//                        }
//                        printf("min x = %d, min y = %d, max x = %d, max y = %d\n", min_x, min_y, max_x, max_y);
//                        imshow("cur", temp);
//                    }
//                }
//                if(motion_detect.findObject(pre, area, temp, found, false)){
//                    cout<< found.tl() << found.br()<<" "<<found.width << " "<<found.height <<"\n";
//                    printf("cost = %dms\n", motion_detect.m_find_cost);
//                    rectangle(temp, found, red, 3);
//                    imshow("cur", temp);
//                    area = found;
//                }
//                if(motion_detect.matchObject(pre, area, cur, found, false)){
//                    printf("cost %d ms\n", motion_detect.m_match_cost);
//                    cout<< found.tl() << found.br()<<" "<<found.width << " "<<found.height <<"\n";
//                    rectangle(temp, found, red, 3);
//                    imshow("cur", temp);
//                    area = found;
//                }

                rectangle(temp, area, blue, 3 );
                rectangle(temp, search_area, red, 2);
                imshow("cur", temp);
            }else{

                area = Rect(sp1, sp2);
                sp3.x = (sp1.x + sp2.x) / 2;
                sp3.y = (sp1.y + sp2.y) / 2;
                cout<< "sp"<<sp1 << sp2<<"\n";
                motion_detect = MotionDetect();
                rectangle(temp, area, blue, 3 );

                search_area = Rect(rp1, rp2);
                cout<<"rp "<<rp1 <<rp2<<"\n";
                rectangle(temp, search_area, red, 2);
                imshow("cur", temp);
                select_frame = frame_count;

            }
            if(wait){
                if(frame_count == wait_frame){
                    wait = false;
                    interval = 0;
                }
                else if(frame_count < wait_frame){
                    interval = 1;
                }else{
                    wait = false;
                }
            }
            printf("frame %d \n", frame_count);

            fflush(stdout);
            char k ;
            if(!wait){
               k = waitKey(interval);
            }else{
                k = -1;
            }
            if(k == 27){
                break;
            }else if (k == ' '){
                interval = 40;
                same = !same;
            }else if(k == 37){
                if(interval > 1)
                    interval --;
            }else if(k == 39){
                interval ++;
            }else if(k == 38){
                interval = 1;
            }else if(k == 40){
                interval = 1000;
            }else if(k == 'm'){
                match = !match;
            }else if(k == 't'){
                track = !track;
                same = !same;
            }


        }
        pre.copyTo(pre_pre);
        cur.copyTo(pre);
    }
    printf("========================\nselect frame = %d, last frame = %d\n", select_frame, frame_count);
    cout<< "sp"<<sp1 << sp2<<"\n";
    cout<<"rp "<<rp1 <<rp2<<"\n======================\n";
}

void Test::testBasic(int argc, const char **argv){
    Mat a = Mat::zeros(4, 4, CV_8U);

    a(Rect(0, 0, 3, 4)).setTo(1);
    cout<<a<<"\n";
    Moments m = moments(a, true);
    printf("m00 = %f, m10 = %f, m01 = %f\n", m.m00, m.m10, m.m01);

}


void Test::testSpeed(int argc, const char **argv){
    VideoCapture cap;
    cap.open(-1);
    Mat img;
    for(;;){
        cap >> img;
        if(img.empty())
            break;
        imencode(".jpg", img, m_img_data);


    }
}

void Test::testSerialPort(int argc, const char **argv){
    CSerial ptz;
    char * tagf = "ttyACM";
    unsigned char set_pwm[20];
    memset(set_pwm,'\0',20);
    int pwm;
    if(ptz.OpenSerial(tagf,0,115200,8,1,1)){
        printf("serial port ok!\n");
    }
    if(ptz.StartSerial()){
            cout << "Start receivings the data" << endl;
    }
    while(1){
            cout << "Please input the pwm value:";
            cin >> pwm;
            ptz.SetPwmToStr(2,pwm,set_pwm);
//            printf("%s\n",set_pwm);
            cout << endl;
            ptz.SendData(set_pwm,12);
    //		sleep(100);
        }


}
